COMMENT ⊗   VALID 00027 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00004 00002	ARMSOL - SUITE OF PROGRAMS INVOLVING ARM KINEMATICS
C00006 00003	SOLVE  - COMPUTES JOINT ANGLES GIVEN A TRANSFORM
C00009 00004		["SOLVE" - CHECK IF ARM OR HAND]
C00012 00005		["SOLVE" - JOINT 1]
C00015 00006		["SOLVE" - JOINTS 2 & 3]
C00017 00007		["SOLVE" - JOINT 5]
C00020 00008		["SOLVE" - DEGENERATE SOLUTION AND JOINT 4]
C00024 00009		["SOLVE" - JOINT 6]
C00027 00010		["SOLVE" LOCAL STORAGE AREA]
C00029 00011	UPDATE - COMPUTES THE ARM TRANSFORM GIVEN THE JOINT ANGLES
C00033 00012		["UPDATE"- HAND OR T6] 
C00035 00013	JACOB  - COMPUTES JACOBIAN MATRIX
C00036 00014		["JACOB" - FORM CT0,CT3,CT6]
C00039 00015		["JACOB" - POSITION VECTORS, AXES OF ROTATION]
C00042 00016		["JACOB" - CROSS PRODUCTS, CLEAN UP]
C00044 00017	T0336  - COMPUTES TRANSFORMATION MATRICIES FOR JTS 1-3 AND 4-6
C00047 00018	T6330  - COMPUTES TRANSFORMATION MATRICIES FOR JTS 6-4 AND 3-0
C00049 00019	R03    - COMPUTES ROTATION MATRIX FOR TRANSFORMATION FROM JTS 1-3
C00051 00020	R36    - COMPUTES ROTATION MATRIX FOR TRANSFORMATION FROM JTS 4-6
C00054 00021	INVERT - INVERTS A TRANSFORMATION STORED BY COLUMNS
C00055 00022	MATMUL - PERFORMS THE OPERATION  T0 ← T1*T2
C00058 00023	MULT   - PERFORMS THE OPERATION  A0 ← A1*A2
C00060 00024	CROSS  - VECTOR CROSS PRODUCT OPERATION V0 ← V1 X V2
C00062 00025	LOCAL STORAGE AREA
C00064 00026	PHYSICAL CONSTANTS FOR YELLOW AND BLUE ARMS
C00066 00027		[BLUE ARM TABLE OF CONSTANTS]
C00068 ENDMK
C⊗;
;ARMSOL - SUITE OF PROGRAMS INVOLVING ARM KINEMATICS

.TITLE ARMSOL

;THE FOLLOWING PROGRAMS ARE  USED TO COMPUTE THE JOINT  ANGLES FOR THE
;SCHEINMAN  MANIPULATORS  GIVEN A  REQUIRED TRANSFORM  OR  VISE VERSA.
;ALSO INCLUDED IS A PROGRAM FOR COMPUTING THE REACTION TORQUES  AT THE
;JOINTS FOR A GIVEN FORCE AND MOMENT COUPLE AT THE HAND.  

;THE  THEORETICAL BASIS  FOR THE  KINEMATIC  SOLUTIONS IMPLEMENTED  IN
;THESE PROGRAMS IS PRESENTED IN STANFORD ARTIFICIAL INTELLIGENCE 
;LABORATORY HAND/EYE GROUP PROGRESS REPORT, DEC 1975 THRU JULY 1976.
;WHEREVER POSSIBLE, THE VARIABLE  NAMES USED IN  THAT DOCUMENT AND IN
;THE FOLLOWING PROGRAMS ARE THE SAME. 


;THE FOLLOWING MECHANISM BITS INDICATE WHICH PHYSICAL DEVICE IS BEING
;USED:
 
YELARM	==1	;YELLOW ARM, NOT INCLUDING HAND
YELHND	==2 	;YELLOW HAND
BLUARM	==4	;BLUE ARM, NOT INCLUDING HAND
BLUHND	==10	;BLUE HAND


;THE FOLLOWING BITS ARE USED BY THE FORCE SENSING ROUTINES TO INDICATE
;IF THE FORCE TRANSFORMATION IS IN TABLE OR HAND COORDINATES.

FTABLE	==400	;TABLE COORDINATES
FHAND	==0	;HAND COORDINATE SYSTEM


;TRANSFORMATION DEFINITIONS

T11==0
T21==4
T31==10
T12==14
T22==20
T32==24
T13==30
T23==34
T33==40
T14==44
T24==50
T34==54
;SOLVE  - COMPUTES JOINT ANGLES GIVEN A TRANSFORM

;GIVEN A TRANSFORM "T", THE REQUIRED JOINT ANGLES ARE DETERMINED IN DEGREES.
;IF ANY JOINT ANGLE IS OUTSIDE OF THE PERMITTED RANGE OF MOVEMENT, THE STOP
;LIMIT ANGLE CLOSEST TO THE PREVIOUS JOINT ANGLES COMPUTED IS RETURNED.  
;IN THE DETERMINATION OF JOINT ANGLE 4, IF THE REQUIRED CHANGE IN ANGLE IS
;GREATER THAN 90 DEGRESS AND LESS THAN 180, THE HAND IS FLIPPED OVER AND THE
;JOINT ANGLE CHANGE BECOMES THETA = THETA-90.  ON COMPLETION OF EXECUTION, 
;REGISTER R0 CONTAINS BITS INDICATING THOSE JOINTS FOR WHICH NO SOLUTION
;COULD BE FOUND WITHIN THE RANGE OF JOINT MOTION.  A SAMPLE CALLING SEQUENCE
;TO "SOLVE" FOLLOWS:
;
;		MOV	#T,R0		;LOAD ADDRESS OF TRANSFORM "T"
;		MOV	#THETA,R1	;PTR TO A TABLE CONTAINING POINTERS TO THE 
;					;  JOINT ANGLES FOR ALL EXISTING ARM JOINTS,
;					;  I.E., THE TABLE MUST HAVE 14 ELEMENTS.
;		MOV	#MECHSM,R2     	;CONSTANT INDICATING WHICH ARM TO USE
;		JSR	PC,SOLVE	;CALLED USING PC
;		TST	R0		;CHECK FOR NUMBER OF NON-EXACT SOLUTIONS
;
;IF THE BLUE OR YELLOW HAND IS SPECIFIED AS THE MECHANISM, THIS ROUTINE
;PUTS THE VALUE POINTED TO BY "T" INTO ITS APPROPRIATE SPOT IN THE "THETA"
;ARRAY.  ALL NUMBERS SHOULD BE IN SINGLE PRECISION FLOATING POINT
;REPRESENATATION.

;EXECUTION TIME:  2200 MICRO SECONDS

;REGISTERS USED:
;
;	R0,R1 PASS ARGUMENTS AND ARE ALTER BY "SOLVE",R2 PASSES ARGUMENTS AND IS
;	  UNALTERED.
;	AC0, AC1, AC2, AND AC3 ARE GARBAGED

;THE OUT OF RANGE BITS THAT ARE RETURNED IN R0 ARE DEFINED AS FOLLOWS:

JT1==	1	;JOINT 1
JT3==	4	;  "   3
JT4==	10	;  "   4
JT5==	20	;  "   5
JT6==	40	;  "   6
;	["SOLVE" - CHECK IF ARM OR HAND]

SOLVE:	CLR	EXACTS		;ASSUME EXACT SOLUTION FOUND
	MOV	R5,-(SP)	;SAVE REGISTER
	BIT	#BLUHND+YELHND,R2	;CHECK IF A HAND IS SPECIFIED
	BEQ	1$		;BRANCH IF NOT
	LDF	(R0),AC0	;GET THE REQUESTED HAND OPENING
	ADD	#12.,R1		;POINT TO THE YELLOW HAND CELL
	BIT	#BLUHND,R2	;CHECK WHICH HAND
	BEQ	.+6
	ADD	#14.,R1		;POINT TO BLUE HAND
	STF	AC0,@(R1)	;SAVE THE HAND OPENING
	JMP	SOLDNE		;ELSE EXIT IMMEDIATELY
1$:	MOV	#YCON,R5	;MUST BE AN ARM, GET PTR TO YELLOW ARM CONSTANTS
	BIT	#YELARM,R2  	;SWITCH CONSTANTS IF BLUE ARM
	BNE	2$
	MOV	#BCON,R5
	ADD	#14.,R1		;POINT TO BLUE ARM JT. ANGLES

;COMPUTE POSITION OF THE END OF THE BOOM: TX,TY,TZ.  ALSO COMPUTE
;SQRT(TX**2+TY**2-S2**2) FOR LATER

2$:	LDF	S6(R5),AC1	;GET OFFSET OF JT. 6:  S6
	NEGF	AC1
	LDF	T13(R0),AC0	;TX ← T14 - T13*S6 - X BASE POS.
	MULF	AC1,AC0
	SUBF	BASEX(R5),AC0	
	ADDF	T14(R0),AC0
	STF	AC0,TX		;SAVE TX
	MULF	AC0,AC0		;AC0 ← TX**2
	LDF 	T23(R0),AC2	;TY ← T24 - T23*S6 - Y BASE COORD.
	MULF	AC1,AC2
	SUBF	BASEY(R5),AC2
	ADDF	T24(R0),AC2
	STF	AC2,TY		;SAVE TY
	MULF	AC2,AC2		;AC0 ← TX**2 + TY**2
	ADDF	AC2,AC0
	MULF	T33(R0),AC1	;TZ ← T34 - T33*S6
	ADDF	T34(R0),AC1
	STF	AC1,TZ		;SAVE TZ
	SUBF	S22(R5),AC0	;AC0 ← TX**2 + TY**2 - S2**2
	CFCC			;CHECK IF RESULT > 0
	BGT	3$
	CLRF	AC0		;IF <0 SET TO 0 AND INDICATE ERROR
	BIS	#JT1,EXACTS	;THIS WILL CAUSE JT 1 TO BE WRONG
3$:	JSR	PC,SQRTF
	NEGF	AC0
	STF	AC0,SQRTXY	;SAVE -SQRT(TX**2+TY**2-S2**29
;	["SOLVE" - JOINT 1]

;COMPUTE JOINT 1 ANGLE

	SUBF	TX,AC0		;TAN TH1/2= (-TX-(TX↑2+TY↑2-S2↑2)↑.5)/(S2+TY)
	LDF	TY,AC1		;INDETERMINANT? IS S2+TY = 0 ?
	ADDF	S2(R5),AC1
	STF	AC1,AC2		;COMPARE TO .001
	ABSF	AC2
	CMPF	C001,AC2
	CFCC
	BLT	5$		;BRANCH IF DENOMINATOR NOT ZERO
	TST	TX		;ELSE CHECK IF INDETERMINATE OR SPECIAL CASE
	BLT	4$ 		;BRANCH IF INDETERMINATE FORM OF EQU. NECESSARY
	LDF	C180,AC0	;ELSE THETA 1 = 180 DEGREES
	BR	JT1CHK		;NOW GO CHECK LIMITS
4$:	LDF	TY,AC0		;TAN TH1/2 = TY/TX
	LDF 	TX,AC1
5$:	TSTF	AC1		;-90 < TH1/2 < 90
	CFCC
	BGE	JT1CPU
	NEGF	AC0
	NEGF	AC1
JT1CPU:	JSR	PC,ATAN2	;COMPUTE THETA 1/2
	MULF	#40400,AC0	;THETA 1
JT1CHK:	CMP	#YELARM,R2    	;CHECK IF YELLOW OR BLUE ARM
	BNE	1$		;BRANCH IF BLUE ARM
	CMPF	STOP1+4(R5),AC0	;IF YELLOW, CHECK IF LESS THAN +90 DEG
	CFCC
	BGE	.+6    		;SKIP IF LESS THAN MAX STOP LIMIT
	SUBF	C360,AC0	;ELSE SUBTRACT 360 DEGREES
     	CMPF	STOP1(R5),AC0	;COMPARE TO MIN STOP LIMIT
	CFCC
	BLE	J1OK		;BRANCH IF WITHIN STOP LIMITS
	BR	2$
1$:	CMPF	STOP1(R5),AC0	;IF BLUE, CHECK IF GREATER THAN -90 DEG
	CFCC
	BLE	.+6 		;SKIP IF GREATER THAN
	ADDF	C360,AC0	;ELSE ADD 360 DEGREES
    	CMPF	STOP1+4(R5),AC0	;COMPARE ANGLE TO MAX STOP LIMIT
	CFCC
	BGE	J1OK		;BRANCH IF WITHIN STOP LIMITS
2$:	BIS	#JT1,EXACTS	;INDICATE NO EXACT SOLUTION
	LDF	STOP1(R5),AC0	;USE CLOSEST STOP LIMIT
	LDF	MIDDY1(R5),AC1	;MID-RANGE ANGLE
	CMPF	@(R1),AC1	;COMPARE TO PREVIOUS
	CFCC
	BLE	J1OK
	LDF	STOP1+4(R5),AC0	;USE MAXIMUM IF CLOSER
J1OK:	STF	AC0,@(R1)+	;SEND OFF NEW JOINT 1 ANGLE
	JSR	PC,SNCOS	;COMPUTE SIN/COS THETA 1 FOR LATER
	STF	AC0,ST1
	STF	AC1,CT1
;	["SOLVE" - JOINTS 2 & 3]

;COMPUTE JOINT 2 ANGLE

	LDF	SQRTXY,AC0	;TAN TH2 = -(TX↑2+TY↑2-S2↑2)↑.5/(TZ-S1)
	LDF	TZ,AC1
	SUBF	S1(R5),AC1
	JSR	PC,ATAN2
	STF	AC0,@(R1)+	;SEND OF NEW JOINT 2 ANGLE.  NO STOP LIMIT
				;  CHECKING SINCE ALL SOLUTIONS WITHIN 0 TO Pi
	JSR	PC,SNCOS 	;COMPUTE SIN/COS OF THETA 2 FOR LATER
	SUBF	C000,AC0	;MAKE SURE SINE THETA 2 NOT ZERO
	STF	AC0,ST2
	STF	AC1,CT2

;COMPUTE JOINT 3 LENGTH

	LDF	TX,AC2		;S3 ← (TX*CT1+TY*ST1)/ST2
	MULF	CT1,AC2
	LDF	TY,AC3
	MULF	ST1,AC3
	ADDF	AC3,AC2
	DIVF	AC0,AC2		;NOW HAVE S3
       	CMPF	STOP3+4(R5),AC2	;COMPARE TO MAX. JOINT EXTENSION
	CFCC
	BGE	1$		;BRANCH IF LESS THAN MAX
	LDF	STOP3+4(R5),AC2	;ELSE SUBSTITUE MAX ALLOWABLE EXT.
	BR	2$
1$:	CMPF	STOP3(R5),AC2	;COMPARE TO MIN. JOINT EXTENSION
	CFCC
	BLE	S3OK		;BRANCH IF IN RANGE
	LDF	STOP3(R5),AC2	;ELSE SUBSTITUTE MIN LENGTH
2$:	BIS	#JT3,EXACTS	;INDICATE EXACT SOLUTION NOT FOUND
S3OK:	STF	AC2,@(R1)+	;SEND BACK JOINT 3
;	["SOLVE" - JOINT 5]

;COMPUTE COS/SIN OF THETA 5 AND CHECK FOR DEGENERACY

	LDF	T13(R0),AC0	;SIN TH5 = SQRT( (-T33*SN2+(T13*CT1+T23*ST1)*CT2)↑2
	MULF	CT1,AC0		;		+ (T13*ST1-T23*CT1)↑2 )
	LDF	T23(R0),AC1
	MULF	ST1,AC1
	ADDF	AC1,AC0
	STF	AC0,T13T23	;SAVE T13*CT1+T23*ST1
	MULF	CT2,AC0
	LDF	T33(R0),AC1
	MULF	ST2,AC1
	SUBF	AC1,AC0
	STF	AC0,ST4		;THIS IS ALMOST SIN(THETA 4)
	MULF	AC0,AC0
	LDF	T13(R0),AC1	;NOW GET T13*ST1-T23*CT1
	MULF	ST1,AC1
	LDF	T23(R0),AC2
	MULF	CT1,AC2
	SUBF	AC2,AC1
	STF	AC1,CT4		;THIS IS ALMOST COS(THETA 4)
	MULF	AC1,AC1
	ADDF	AC1,AC0
	JSR	PC,SQRTF	;NOW HAVE SIN(THETA 5)
	LDF	T13T23,AC1 	;COS TH5 = T33*CT2+(T13*CT1+T23*ST1)*ST2
	MULF	ST2,AC1
	LDF	T33(R0),AC2
	MULF	CT2,AC2
	ADDF	AC2,AC1
	JSR	PC,ATAN2
	ABSF	AC0		;WANT +180 NOT -180
	TST	@2(R1)		;CHECK SIGN OF PREVIOUS THETA 5
	BGE	.+4
	NEGF	AC0		;SET SIGN OF CURRENT ANGLE THE SAME
	CMPF	STOP5+4(R5),AC0	;COMPARE TO MAXIMUM STOP LIMIT
	CFCC
	BGE	1$		;BRANCH IF ANGLE LESS THAN STOP LIMIT
	LDF	STOP5+4(R5),AC0	;ELSE REPLACE ANGLE BY STOP LIMIT
	BR	2$
1$:	CMPF	STOP5(R5),AC0	;COMPARE ANGLE TO MIN STOP LIMIT
	CFCC
	BLE	JT5OK		;BRANCH IF ANGLE IN RANGE
	LDF	STOP5(R5),AC0	;ELSE REPLACE ANGLE BY MIN STOP LIMIT
2$:	BIS	#JT5,EXACTS	;INDICATE NO EXACT SOLUTION FOUND
JT5OK:	STF	AC0,@2(R1)	;SEND BACK ANGLE
	STF	AC0,AC1		;CHECK FOR DEGENERATE CASE, TH5 = 0
	ABSF	AC1
	CMPF	C01,AC1	
	CFCC
	BLT	NOTDGN		;BRANCH IF NOT DEGENERATE SOLUTION 
;	["SOLVE" - DEGENERATE SOLUTION AND JOINT 4]

;DEGENERATE SOLUTION:  TH5 = 0, TH4 REMAINS THE SAME, SPECIAL 
;EQUATION FOR TH6

	LDF	T31(R0),AC0	;SIN(TH6+TH4)=-T31/ST2
	LDF	T32(R0),AC1	;COS(TH6+TH4)=-T32/ST2
	JSR	PC,ATAN2	;COMPUTE TH6+TH4
	SUBF	@(R1)+,AC0	;TH6 = ATAN -TH4
	TST	(R1)+
	CMPF	STOP6+4(R5),AC0	;MIGHT BE OFF BY 360 DEG
	CFCC			;GREATER THAN MAXIMUM STOP LIMIT?
	BGE	1$
	SUBF	C360,AC0	;YES, SUBTRACT 360 DEGREES
	JMP	JT6CHK		;GO CHECK IF JT 6 IN RANGE
1$:	CMPF	STOP6(R5),AC0	;LESS THAN MINIMUM STOP LIMIT?
	CFCC
	BLE	2$
	ADDF	C360,AC0	;YES, ADD 360 DEGREES
2$:	JMP	JT6CHK

;NON-DEGENERATE SOLUTION: COMPUTE THETA 4
	
NOTDGN:	LDF	ST4,AC0		;SIN(THETA 4)=-T33*SN2+(T13*CT1+T23*ST1)*CT2/ST5
	LDF	CT4,AC1		;COS(THETA 4)=(T13*ST1-T23*CT1)/ST5
	TST	@2(R1)		;DONT DIVID, JUST CORRECT FOR SIGN(ST5)
	BPL	1$
	NEGF	AC0
	NEGF	AC1
1$:	JSR	PC,ATAN2	;COMPUTE THETA 4
	STF	AC0,AC1		;DIFFERENCE BETWEEN OLD AND NEW ANGLE
	SUBF	@(R1),AC1
	CMPF	C180,AC1	;DIFFERENCE > 180 DEG ?
	CFCC
	BGE	2$		;BRANCH IF LESS 
	SUBF	C360,AC0	;ELSE SUBT. 360 DEG - GO THE SHORTEST DIRECTION
	SUBF	C360,AC1
	BR	3$
2$:	CMPF	CM180,AC1	;DIFFERENCE > -180 DEG ?
	CFCC
	BLE	3$		;BRANCH IF GREATER 
	ADDF	C360,AC0	;ELSE ADD 360 DEG - GO THE SHORTEST DIRECTION
	ADDF	C360,AC1
3$:	CMPF	C90,AC1		;DIFF. < 90 DEG ?
	CFCC
	BGE	4$		;BRANCH IF IT IS
	SUBF	C180,AC0	;ELSE GO THE OTHER DIRECTION AND FLIP HAND
	BR	FLIPJ4
4$:	CMPF	CM90,AC1	;DIFF. > -90 DEG ?
	CFCC
	BLE	JT4CHK		;BRANCH IF IT IS
	ADDF	C180,AC0	;ELSE GO THE OTHER DIRECTION
FLIPJ4:	NEGF	@2(R1)		;FLIP HAND OVER
JT4CHK:	CMPF	STOP4(R5),AC0	;COMPARE ANGLE TO MIN STOP LIMIT
	CFCC
	BLE	1$		;BRANCH IF GREATER THAN MIN
	ADDF	C180,AC0	;ELSE ADD 180 DEG AND FLIP HAND
	BR	2$
1$:	CMPF	STOP4+4(R5),AC0	;COMPARE TO MAX STOP LIMIT
	CFCC
	BGE	JT4OK		;BRANCH IF JT 4 IN RANGE
	SUBF	C180,AC0	;ELSE SUBT 180 DEG AND FLIP HAND
2$:	NEGF	@2(R1)
JT4OK:	STF	AC0,@(R1)+	;SEND BACK THE NEW JOINT 4 ANGLE
;	["SOLVE" - JOINT 6]

;COMPUTE THETA 6

	LDF	ST1,AC2		;SIN(THETA 6)=((T12*CT1+T22*ST1)*ST2
	LDF	CT1,AC3		;             +T32*CT2)/ST5
	LDF	T12(R0),AC0
	MULF	AC3,AC0
	LDF	T22(R0),AC1
	MULF	AC2,AC1
	ADDF	AC1,AC0
	MULF	ST2,AC0
	LDF	T32(R0),AC1
	MULF	CT2,AC1
	ADDF	AC1,AC0
	LDF	T21(R0),AC1	;COS(THETA 6)=-((T21*ST1+T11*CT1)*ST2
	MULF	AC2,AC1		;              +T31*CT2)/ST5
	LDF	(R0),AC2
	MULF	AC3,AC2
	ADDF	AC2,AC1
	MULF	ST2,AC1
	LDF	T31(R0),AC2
	MULF	CT2,AC2
	TST	@(R1)		;DON'T DIVID BY ST5, JUST CORRECT SIGN
	ADDF	AC2,AC1
	BGE	1$
	NEGF	AC0		;IF SIN(TH5)<0 USE -SIN(TH6)
	BR	.+4
1$:	NEGF	AC1		;ELSE USE -COS(TH6)
	TST	(R1)+		;POINT TO OLD THETA 6
	JSR	PC,ATAN2	;COMPUTE THETA 6
JT6CHK:	CMPF	STOP6(R5),AC0	;CHECK IF ANGLE GREATER THAN MIN.
	CFCC
	BLE	1$		;BRANCH IF GREATER
	ADDF	C180,AC0	;ELSE ADD 180 DEG AND FLIP HAND
	BR	2$
1$:	CMPF	STOP6+4(R5),AC0	;CHECK IF ANGLE LESS THAN MAXIMUM
	CFCC
	BGE	JT6OK		;BRANCH IF JT6  IN RANGE
	SUBF	C180,AC0	;ELSE SUBT 180 DEG AND FLIP HAND
2$:	STF	AC0,@(R1)	;SAVE NEW THETA 6
	NEGF	@-(R1)		;FLIP HAND BY COMPLEMENTING TH5
	LDF	@-(R1),AC0	;ALTER THETA 4 BY 180 DEGREES
	ADDF	C180,AC0	;TRY ADDING 180 DEGREES
	CMPF	STOP4+4(R5),AC0	;COMPARE TO MAXIMUM
	CFCC
	BGE	3$		;BRANCH IF STILL IN RANGE
	SUBF	C360,AC0	;ELSE TRY SUBT 180 DEGREES
	CMPF	STOP4(R5),AC0
	CFCC
	BLE	3$		;BRANCH IF JT 4 NOW IN RANGE
	LDF	STOP4(R5),AC0	;ELSE JUST USE STOP LIMIT
	BIS	#JT4,EXACTS	;INDICATE NO EXACT SOLUTION
3$:	STF	AC0,@(R1)	;SAVE NEW THETA 4
	BR	.+6
JT6OK:	STF	AC0,@(R1)	;SAVE NEW THETA 6

;EXIT CLEANLY

SOLDNE:	MOV	(SP)+,R5	;RESTORE REGISTERS
	MOV	EXACTS,R0	;LEAVE ERROR BITS FOR CALLER
	RTS	PC

;END OF "SOLVE"
;	["SOLVE" LOCAL STORAGE AREA]

TX:	.BLKW	2	;LOCATION OF THE END OF THE BOOM
TY:	.BLKW	2
TZ:	.BLKW	2
SQRTXY:	.BLKW	2	;-SQRT(TX**2+TY**2-S2**2)
T13T23:	.BLKW	2	;T13*CT1+T23*ST1
ST1:	.BLKW	2	;SIN/COS THETA 1
CT1:	.BLKW	2
ST2:	.BLKW	2	;SIN/COS THETA 2
CT2:	.BLKW	2
ST4:	.BLKW	2	;SIN/COS THETA 4 (ALMOST)
CT4:	.BLKW	2

EXACTS:	0		;INDICATES JTS WITH NON-EXACT SOLUTIONS


;CONSTANTS

C01:	.WORD	 37314,146315	; .1000000    
C001:	.WORD	 35603, 11157	; .1000000@-2 
C360:	.WORD	 42264,     0	; 360.0000    
C90:	.WORD	 41664,     0	; 90.00000    
CM90:	.WORD	141664,     0	;-90.00000    
C180:	.WORD	 42064,     0	; 180.0000    
CM180:	.WORD	142064,	    0	;-180.0000
C000:	.WORD	 31453,146167	; .1000000@-7 
;UPDATE - COMPUTES THE ARM TRANSFORM GIVEN THE JOINT ANGLES

;GIVEN THE JOINT ANGLES, THE RESULTING HAND POSITION AND ORIENTATION ARE
;DETERMINED AND STORED IN A GIVEN TRANSFORM "T".  THE TRANSFORM IS A 4 BY 4 
;MATRIX STORED BY COLUMNS WITH EACH VALUE REPRESENTED IN TABLE COORDINATES.
;NO INITIALIZATION NEEDS TO BE DONE ON THE T SINCE ALL THE ELEMENTS WILL BE
;UPDATED, EVEN THOUGH THE LAST ROW IS ALWAYS 0,0,0,1 .  A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
;		MOV	#T,R0		;LOAD TRANSFORM ADDRESS IN R0
;		MOV	#THETA,R1	;PTR TO A TABLE CONTAINING POINTERS TO THE 
;					;  JOINT ANGLES FOR ALL EXISTING ARM JOINTS,
;					;  I.E., THE TABLE MUST HAVE 14 ELEMENTS.
;		MOV	#MECHSM,R2     	;CONSTANT INDICATING WHICH ARM TO USE
;		JSR	PC,UPDATE	;CALLED USING PC
;
;THE TRANSFORM WILL BE OF THE FOLLOWING FORM:
;
;		|  T1   T5   T9   T13  |
;		|  T2   T6   T10  T14  |
;		|  T3   T7   T11  T15  |
;		|   0    0    0   1.0  |
;
;THE PROCEDURE CONSISTS OF GENERATION OF TWO MATRICES T1 AND T2, THE TRANSFORM
;FROM SHOULDER TO WRIST AND FROM WRIST TO HAND RESPECTIVELY, THEN MULTIPLYING
;THE TWO TO GIVE THE DESIRED TRANSFORM.  
;
;THIS PROCEDURE MODIFIED ON JUNE 9TH, 1976 TO THE EXTENT THAT THE FIRST
;COL OF T2 IS NOT COMPUTED, AND THUS ONLY THE THIRD THREE COLS OF
;T ARE COMPUTED.  THE FIRST COLUMN IS THEN FOUND BY TAKING THE CROSS PRODUCT
;OF THE SECOND AND THIRD COLUMNS.
;
;IF THE BLUE OR YELLOW HAND IS SPECIFIED AS THE MECHANISM, THE HAND OPENING  
;POINTED TO BY THE "THETA" ARRAY IS RETURNED IN THE LOCATION POINTED TO BY
;"T".  ALL NUMBERS SHOULD BE IN SINGLE PRECISION FLOATING POINT 
;REPRESENATATION.

;EXECUTION TIME:	2080 microsecs

;REGISTERS USED:     
;
;	R0 - T TRANSFORM/LINK NUMBER
;	R1 - THETA LIST/ARM CONSTANTS LIST POINTER
;	R2 - BLUE/YELLOW ARM FLAG
;	AC0,AC1,AC2,AC3,AC4,AC5 GARBAGED
;	R0,R1 GARBAGED

;DEFINITIONS:

SN==%0
CS==%1
SN1==%4
CS1==%5
SN2==%0
CS2==%1
SN4==%3
CS4==%4
CS5==%5
;	["UPDATE"- HAND OR T6] 

UPDATE:	MOV	R2,-(SP)	;SAVE REGISTERS
	MOV	R3,-(SP)
	MOV	R4,-(SP)
	MOV	R5,-(SP)
	BIT	#YELARM+BLUARM,R2	;CHECK THAT ONE OF THE ARM REQUESTED
	BNE	1$

;SECTION TO RETURN HAND OPENINGS

	ADD	#12.,R1		;GET POINTER TO YELLOW HAND OPENING
	BIT	#YELHND,R2	;CHECK WHICH HAND WE ARE DEALING WITH
	BNE	.+6		;SKIP IF ITS THE YELLOW HAND MECH. BIT
	ADD	#14.,R1		;ELSE GET THE BLUE HAND POINTER
	LDF	@(R1),AC0	;GET THE PREVIOUS HAND OPENING
	STF	AC0,(R0)	;RETURN THE CORRECT HAND OPENING AND EXIT
	BR	3$

;MUST BE AN ARM, COMPUTE T0_3 & T3_6 AND MULTIPLY THEM TOGETHER

1$:	MOV	#YCON,R3	;ADDR OF YELLOW ARM PHYSICAL CONSTANTS
	BIT	#YELARM,R2    	;CHECK IF YELLOW OR BLUE ARM
	BNE	2$		;SKIP IF YELLOW ARM
	MOV	#BCON,R3	;ELSE GET ADDR OF BLUE ARM CONSTANTS
	ADD	#14.,R1		;POINT TO BLUE ARM JOINT ANGLES
2$:	CLR	R4		;COMPUTE T0_3 & T3_6: ADD IN BASEX,BASEY
	JSR	PC,T0336U
	MOV	#T311,R1	;RESULT ← T3*T6
	MOV	#T011,R2
	JSR	PC,MATMUL

;RESTORE REGISTERS AND EXIT

3$:	MOV	(SP)+,R5	;RESTORE REGISTERS
	MOV	(SP)+,R4
	MOV	(SP)+,R3
	MOV	(SP)+,R2
	RTS	PC		;EXIT

;END OF "UPDATE"
;JACOB  - COMPUTES JACOBIAN MATRIX

;COMPUTES JACOBIAN MATRIX FOR A ARBITRARY TRANSFORMATIION.
;
; WE NEED:
;	1.  PTR TO STORAGE FOR JACOBIAN MATRIX
;	2.  PTR TO TABLE OF PTRS TO JOINT ANGLES
;	3.  PTR TO ORC OR 6RC MATRIX
;	4.  BLUE/YELLOW INDICATOR
;	5.  ORC OR 6RC INDICATOR
;	6.  STORAGE FOR CA6, CA3, CA0?

;	R0 ← PTR TO FORCE TRANSFORMATION: C
;	R1 ← PTR TO TABLE OF PTRS TO JOINT ANGLES
;	R2 ← ARM/TABLE/HAND BITS
;	R3 ← PTR TO ARRAY TO RETURN JACOBIAN IN
;ON COMPLETION
;	T0 ← CT0	T3 ← CT3	T6 ← CT6

;EXECUTION TIME:  4 MSEC

;REGISTERS USED:
;	R0,R1,R2,R3 PASS ARGUMENTS AND R0,R1 ARE GARBAGED
;	["JACOB" - FORM CT0,CT3,CT6]

JACOB:	MOV	R5,-(SP)	;SAVE REGISTERS
	MOV	R4,-(SP)
	MOV	R3,-(SP)	;RETURN JACOBIAN IN HERE
	MOV	R2,-(SP)

	MOV	#YCON,R3	;ADDR OF YELLOW ARM PHYSICAL CONSTANTS
	BIT	#YELARM,R2    	;CHECK IF YELLOW OR BLUE ARM
	BNE	1$		;SKIP IF YELLOW ARM
	MOV	#BCON,R3	;ELSE GET ADDR OF BLUE ARM CONSTANTS
	ADD	#14.,R1		;POINT TO BLUE ARM JOINT ANGLES

1$:	MOV	S1+2(R3),-(SP)	;SAVE S1
	MOV	S1(R3),-(SP)
	MOV	R0,-(SP)	;SAVE C
	BIT	#FTABLE,R2	;FORCE SENSING IN HAND OR TABLE COOR.
	BNE	2$
	
;RELATIVE HAND TRANSFORMATION GIVEN:6TC, COMPUTE 3T0 AND 6T3

	JSR	PC,T6330	;T3 ← 6T3 , T6 ← 3T0
	MOV	#T611,-(SP)	;T6 ← CT6 ← INVERSE(6TC)
	MOV	#T011,-(SP)	;T0 ← CT0 ← CT3*3T0
	BR	GET036

;TABLE TRANSFORMATION GIVEN:0TC, COMPUTE 0T3 AND 3T6

2$:	MOV	#10,R4		;T3 ← 0T3 , T6 ← 3T6
	JSR	PC,T0336	;DONT ADD IN BASEX OR BASEY
	MOV	#T011,-(SP)	;T0 ← CT0 ← INVERSE(0TC)
	MOV	#T611,-(SP)	;T6 ← CT6 ← CT3*3T6

;COMPUTE T0←CT0, T3←CT3, T6←CT6

GET036:	MOV	R0,R1		;C
	MOV	#T312,R0	;T3 ← CT3 ← C*T3
	MOV	#T312,R2	;FORM COL 2,3,4 OF NEW T3
	MOV	#3,R3
	JSR	PC,MULROW
	MOV	#T311,R0	;COMPUTE T3(I,1) BY CROSSING T3(I,2)
	MOV	#T312,R1	;   WITH T3(I,3)
	MOV	#T313,R2
	JSR	PC,CROSS
	MOV	(SP)+,R0	;(R0) ← T3*T6
	MOV	#T311,R1
	MOV	#T611,R2
	JSR	PC,MATMUL
	MOV	(SP)+,R0	;(R0) ← INVERSE(C)
	MOV	(SP)+,R1	;C
	MOV	#3,R2
1$:	MOV	(R1)+,(R0)+	;TRANSPOSE A COL INTO A ROW
	MOV	(R1)+,(R0)+
	MOV	(R1)+,10(R0)
	MOV	(R1)+,12(R0)
	MOV	(R1)+,24(R0)
	MOV	(R1)+,26(R0)
	SOB	R2,1$
	ADD	#30,R0
	CLRF	(R0)+		;X,Y,Z ← 0
	CLRF	(R0)+
	CLRF	(R0)
;	["JACOB" - POSITION VECTORS, AXES OF ROTATION]

;COMPUTE POSITION VECTORS FROM AXES OF ROTATION TO HAND AND DIRECTION
;COSINES OF AXES OF ROTATION, ALL IN COORDINATE SYSTEM C

FRMJAC:	LDF	(SP)+,AC3	;NEED S1 TO DETERMINE A POINT ON Z2
	MOV	2(SP),R5	;SAVE JACOBIAN IN HERE
	MOV	#T614,R0	;CPX6
	MOV	#T014,R1	;CPX0
	MOV	#T013,R2	;CAX0
	MOV	#3,R3
1$:	LDF	(R0)+,AC0	;POSITION VECTOR FOR JT 1: P1 ← CP6-CP0
	SUBF	(R1)+,AC0
	STF	AC0,(R5)+
	LDF	(R2),AC1	;   "       "     "  JT 2: P2 ← P1-S1*CA0
	MULF	AC3,AC1
	MOV	(R2)+,10(R5)	;ROTATION AXIS OF JT 1: Z1 ← CA0
	MOV	(R2)+,12(R5)
	SUBF	AC1,AC0
	STF	AC0,24(R5)
	SOB	R3,1$

	ADD	#30,R5		;STORE ROTATION AXIS OF JT 2 HERE: Z2
	MOV	#T311,R0	;Z2 ← -CT3 ( 1ST COLUMN OF MATRIX CT3 )
	MOV	#T313,R1	;JT 3 JACOBIAN HAS ONLY A FORCE COMPONENT: CA3
	MOV	#3,R2
2$:	LDF	(R0)+,AC0	;Z2 ← -CT3
	NEGF	AC0
	STF	AC0,(R5)+
	LDF	(R1)+,AC0	;CA3
	STF	AC0,10(R5)
	CLRF	24(R5)		;CONTRIBUTION OF JT3 TO ROTATIONS: Z3 ← 0
	SOB	R2,2$

	ADD	#30,R5		;TEMPORARILY SAVE POSITION VECTOR JT 4 IN HERE
	MOV	#T614,R0	;CPX6
	MOV	#T314,R1	;CPX3
	MOV	#T313,R2	;CAX3
	MOV	#3,R3
3$:	LDF	(R0)+,AC0	;POSITION VECTOR FOR JT 4: P4 ← CP6-CP3
	SUBF	(R1)+,AC0
	MOV	(R2)+,14(R5)	;ROTATION AXIS OF JT 4: Z4 ← CA3
	MOV	(R2)+,16(R5)
	STF	AC0,(R5)+
	STF	AC0,24(R5)	;POSITION VECTOR FOR JT 5: P5 ← P4
	SOB	R3,3$

	ADD	#30,R5		;PUT DIRECTION COSINES OF Z5 IN HERE
	LDF	SINT6,AC2	;NEED SIN/COS THETA 6
	LDF	COST6,AC3
	MOV	#T611,R0	;1ST COL OF CT6
	MOV	#T612,R1	;2ND "   "   "
	MOV	#T613,R2	;CAX6
	MOV	#3,R3
4$:	LDF	(R0)+,AC0	;COMPONENT OF Z5 ← T6I1*ST6+T6I2*CT6
	MULF	AC2,AC0
	CLR	14(R5)		;POSITION VECTOR FOR JT6: P6 ← 0
	CLR	16(R5)
	LDF	(R1)+,AC1
	MULF	AC3,AC1
	MOV	(R2)+,30(R5)	;ROTATION AXIS OF JT 6: Z6 ← CAX6
	MOV	(R2)+,32(R5)
	ADDF	AC1,AC0
	STF	AC0,(R5)+
	SOB	R3,4$
;	["JACOB" - CROSS PRODUCTS, CLEAN UP]

;COMPUTE COMPONENTS OF LINEAR MOTION BY TAKING CROSS PRODUCTS.  NOTE
;THAT JOINT 6 DOES NOT CONTRIBUTE TO LINEAR MOTION AND JOINT 3 NEEDS
;NO CROSS PRODUCT SINCE IT IS A PRISMATIC JOINT.

CROSSS:	MOV	R5,R0		;POSITION COMPONENTS JT 1: Z1 X P1
	SUB	#170,R0		;1ST ROW, 1ST COL OF JACOBIAN
	MOV	R0,R1
	ADD	#14,R1		;Z1
	MOV	R0,R2		;P1
	JSR	PC,CROSS
	ADD	#20,R0		;POSITION COMPONENTS JT 2: Z2 X P2
	ADD	#20,R1		;Z2
	ADD	#20,R2		;P2
	JSR	PC,CROSS
	ADD	#50,R0		;POSITION COMPONENTS JT 4: Z4 X P4
	ADD	#50,R1		;Z4
	ADD	#50,R2		;P2
	JSR	PC,CROSS
	ADD	#20,R0		;POSITION COMPONENTS JT 5: Z5 X P5
	ADD	#20,R1		;Z5
	ADD	#20,R2		;P5
	JSR	PC,CROSS

;RESTORE REGISTERS AND EXIT

	MOV	(SP)+,R2	;RESTORE REGISTERS
	MOV	(SP)+,R3
	MOV	(SP)+,R4
	MOV	(SP)+,R5
	RTS	PC		;EXIT

;END OF "JACOB"
;T0336  - COMPUTES TRANSFORMATION MATRICIES FOR JTS 1-3 AND 4-6

;"T0336" COMPUTES THE TRANSFORMATION MATRIX FROM THE BASE SYSTEM (0)
;TO THE END OF JOINT 3 AND THE MATRIX FROM THE COORDINATE SYSTEM OF
;JOINT 3 TO THE END OF JOINT 6.  ON COMPLETION, THE MATRIX FROM 0 TO 3
;WILL BE STORED IN THE ARRAY "T3" AND THE MATRIX FROM 3 TO 6 WILL
;BE STORED IN "T6".  IF R4 IS 0 THE X,Y LOCATION OF THE BASE OF THE
;ARM WILL BE ADDED IN, OTHERWISE R4 SHOULD BE SET TO '10

T0336U:	MOV	#T36PTU,R2	;TABLE OF PTRS TO T0_3&T3_6
	BR	.+6

T0336:	MOV	#T36PTR,R2	;TABLE OF POINTERS TO T0_3&T3_6
	JSR	PC,R03		;CONSTRUCT UPPER LEFT 3x3 OF T0_3
	LDF	@(R1)+,AC0	;T0_3(3,4)=CS2*S3+S1
	MULF	AC0,CS2
	ADDF	(R3)+,AC1	;GET S1
	STF	AC1,T334
	MULF	AC0,AC2		;T0_3(1,4)=CS1*SN2*S3-SN1*S2
	MULF	AC0,AC3		;T0_3(2,4)=SN1*SN2*S3+CS1*S2
	LDF	(R3)+,AC1	;GET S2
	STF	AC1,AC0
	MULF	SN1,AC0
	ADD	R4,R3		;SKIP BASEX & BASEY IF REQUESTED
	TST	R4
	BNE	.+4
	ADDF	(R3)+,AC2	;+ BASEX
	SUBF	AC0,AC2
	STF	AC2,T314
	MULF	CS1,AC1
	TST	R4
	BNE	.+4
	ADDF	(R3)+,AC3	;+ BASEY
	ADDF	AC1,AC3
	STF	AC3,T324
	JSR	PC,R36		;CONSTRUCT UPPER LEFT 3x3 OF T3_6
	STF	AC0,AC1		;T3_6(3,4)=CS5*S6
	MULF	CS5,AC1
	STF	AC1,@(R2)+
	STF	AC0,AC1		;T3_6(2,4)=SN4*ST5*S6
	MULF	@(R2)+,AC1
	STF	AC1,@(R2)+
	MULF	@(R2)+,AC0	;T3_6(1,4)=CS4*ST5*S6
	STF	AC0,@(R2)
	RTS	PC


;END OF "T0336"
;T6330  - COMPUTES TRANSFORMATION MATRICIES FOR JTS 6-4 AND 3-0

;"T6330" COMPUTES THE INVERSE TRANSFORMATION MATRICIES FROM THOSE
;COMPUTED BY "T0336".  ON COMPLETION, THE MATRIX FROM JOINT 6 TO 3
;IS STORED IN "T3" AND THE MATRIX FROM 3 TO 0 IS STORED IN "T6".
;NOTE THAT THE CORRECTIONS FOR THE X-Y LOCATION OF THE BASE OF
;THE ARMS IS NOT ADDED INTO THE MATRIX TRANSFORMATIONS.

T6330:	MOV	#T63PTR,R2	;TABLE OF POINTERS TO T3_0&T6_3
	JSR	PC,R03		;CONSTRUCT UPPER LEFT 3X3 OF T3_0
	LDF	(R3)+,AC2	;T3_0(2,4)=SN2*S1
	MULF	AC2,SN2
	MOV	(R3)+,T614	;T3_0(1,4)=S2
	STF	SN2,T624
	MULF	AC2,CS2		;T3_0(3,4)=-CS2*S1-S3
	MOV	(R3),T614+2
	ADDF	@(R1)+,CS2
	ADD	#12,R3		;DONT ADD IN BASEX & BASEY
	NEGF	CS2
	STF	CS2,T634
	JSR	PC,R36		;CONSTRUCT UPPER LEFT 3X3 OF T6_3
	CLRF	T314		;T6_3(1,4)=0
	CLRF	T324		;T6_3(2,4)=0
	NEGF	AC0		;T6_3(3,4)=-S6
	STF	AC0,T334
	RTS	PC

;END OF "T6330"
;R03    - COMPUTES ROTATION MATRIX FOR TRANSFORMATION FROM JTS 1-3

;ON COMPLETION, THIS ROUTINE WILL HAVE STORED THE ELEMENTS OF R0_3(1:3,1:3)
;INTO THE ARRAY POINTED TO BY R2.  IN ADDITION, THE FLOATING POINT REGISTERS
;WILL CONTAIN THE FOLLOWING VALUES:
;
;	AC0= SN2	AC1= CS2	AC2= CS1*SN2
;	AC3= SN1*SN2	AC4= SN1	AC5= CS1

R03:	LDF	@(R1)+,AC0	;COMPUTE SIN/COS THETA 1
	JSR	PC,SNCOS
	STF	SN,@(R2)+	;T0_3(1,1)=SN1
	STF	CS,@(R2)+	;T0_3(2,1)=-CS1
	STF	SN,SN1		;AND SAVE FOR LATER
	STF	CS,CS1
	LDF	@(R1)+,AC0	;COMPUTE SIN/COS THETA 2
	JSR	PC,SNCOS
	STF	SN,@(R2)+	;T0_3(3,2)=-SN2
	STF	CS2,@(R2)+	;T0_3(3,3)=CS2
	STF	CS2,AC2		;T0_3(1,2)=CS1*CS2
	MULF	CS1,AC2
	ADD	#100000,@-6(R2)	;CORRECT SIGN( T0_3(2,1) )
	STF	AC2,@(R2)+
	STF	CS2,AC2		;T0_3(2,2)=SN1*CS2
	MULF	SN1,AC2
	ADD	#100000,@-6(R2)	;CORRECT SIGN( T0_3(3,2) )
	STF	AC2,@(R2)+
	LDF	SN2,AC2		;T0_3(1,3)=CS1*SN2
	MULF	CS1,AC2
	STF	AC2,@(R2)+
	LDF	SN1,AC3		;T0_3(2,3)=SN1*SN2
	MULF	SN2,AC3
	STF	AC3,@(R2)+
	CLRF	@(R2)+		;T0_3(3,1)=0
	RTS	PC

;END OF "R03"
;R36    - COMPUTES ROTATION MATRIX FOR TRANSFORMATION FROM JTS 4-6

;ON COMPLETION, THIS ROUTINE WILL HAVE STORED THE ELEMENTS OF R3_6(1:3,1:3)
;INTO THE ARRAY POINTED TO BY R2.  SINCE THE ELEMENT R3_6(1:1) IS NEVER
;NEED BY ANY OF THE CALLING ROUTINES, THIS IS THE ONLY MATRIX ELEMENT THAT
;IS NOT COMPUTED.  AT THE COMPLETION OF THIS ROUTINE, THE FLOATING POINT
;REGISTERS CONTAIN THE FOLLOWING VALUES:
;		AC0= S6		AC5= CS5

R36:	LDF	@(R1)+,AC0	;COMPUTE SIN/COS THETA 4
	JSR	PC,SNCOS
	STF	SN,-(SP)	;SAVE SIN/COS
	STF	CS,CS4
	LDF	@(R1)+,AC0	;COMPUTE SIN/COS THETA 5
	JSR	PC,SNCOS
	STF	CS,@(R2)+	;T3_6(3,3)=CS5
	STF	SN,AC2		;T3_6(2,3)=SN4*ST5
	MULF	(SP),AC2
	STF	AC2,@(R2)+
	STF	SN,AC2		;T3_6(1,3)=CS4*ST5
	MULF	CS4,AC2
	STF	AC2,@(R2)+
	STF	SN,-(SP)	;SAVE SIN/COS THETA 5
	STF	CS,CS5
	LDF	@(R1)+,AC0	;COMPUTE SIN/COS THETA 6
	JSR	PC,SNCOS
	STF	SN,SINT6	;JACOB NEEDS THESE
	STF	CS,COST6
	LDF	(SP)+,AC2	;T3_6(3,2)=ST5*ST6
	STF	AC2,AC3
	MULF	SN,AC2
	STF	AC2,@(R2)+
	MULF	CS,AC3		;T3_6(3,1)=-ST5*CT6
	STF	AC3,@(R2)
	LDF	(SP)+,AC2	;NEED COMBINATIONS OF SIN/COS T4&T6
	STF	AC2,AC3
	MULF	SN,AC2		;SN4*ST6
	ADD	#100000,@(R2)+	;COMPLEMENT T3_6(3,1)
	MULF	CS,SN4		;SN4*CT6
	MULF	CS4,SN		;CS4*ST6
	MULF	CS4,CS		;CS4*CT6
	MULF	CS5,AC2		;T3_6(2,2)=-SN4*CS5*ST6+CS4*CT6
	SUBF	AC1,AC2
	STF	AC2,@(R2)
	STF	AC3,AC2		;T3_6(2,1)=SN4*CS5*CT6+CS4*ST6
	MULF	CS5,AC2
	ADD	#100000,@(R2)+	;COMPLEMENT T3_6(2,2)
	ADDF	AC0,AC2
	STF	AC2,@(R2)+
	MULF	CS5,AC0		;T3_6(1,2)=-SN4*CT6-CS4*CS5*ST6
	ADDF	AC3,AC0
	NEGF	AC0
	STF	AC0,@(R2)+
	LDF	(R3)+,AC0	;NEXT ROUTINE NEEDS S6
	RTS	PC

;END OF "R36"
;INVERT - INVERTS A TRANSFORMATION STORED BY COLUMNS
;
;	R0 ← PTR TO STORAGE FOR INVERTED TRANSFORMATION
;	R1 ← PTR TO TRANSFORMATION TO BE INVERTED
;

INVERT:	MOV	#3,R2		;COUNTER
	LDF	44.(R1),AC0
	STF	AC0,AC4
	LDF	40.(R1),AC3
	LDF	36.(R1),AC2
1$:	LDF	(R1)+,AC0	;NEW T(I,4) ← -OLD T(J,I)*T(J,4)
	STF	AC0,(R0)
	MULF	AC2,AC0
	ADD	#12.,R0
	LDF	(R1)+,AC1
	STF	AC1,(R0)
	MULF	AC3,AC1
	ADD	#12.,R0
	ADDF	AC1,AC0
	LDF	(R1)+,AC1
	STF	AC1,(R0)
	MULF	AC4,AC1
	SUB	#20.,R0
	ADDF	AC1,AC0
	NEGF	AC0
	STF	AC0,32.(R0)
	SOB	R2,1$
	RTS	PC

;END OF "INVERT"
;MATMUL - PERFORMS THE OPERATION  T0 ← T1*T2

;WON'T WORK IF LOC(T0) = LOC(T1)
;
;	R0 ← PTR TO ARRAY FOR RESULT: T0
;	R1 ← PTR TO T1
;	R2 ← PTR TO T2

MATMUL:	JSR	PC,MULSHR	;FORM COL 2,3,4
	SUB	#36.,R0		;COMPUTE T1(I,1) BY CROSSING T1(I,2)
	MOV	R0,R1		;   WITH T1(I,3)
	ADD	#12.,R1
	MOV	R1,R2
	ADD	#12.,R2
	JSR	PC,CROSS
	RTS	PC


MULROT:	ADD	#12.,R0		;FORM COL 1,2,3,4 OF TRANS X ROTATION TRANS
	ADD	#12.,R2
	MOV	#2,R3		
	JSR	PC,MULCOL
	ADD	#36.,R1		;LAST ROW OF T0 = T1
	LDF	(R1)+,AC0
	STF	AC0,(R0)+
	LDF	(R1)+,AC0
	STF	AC0,(R0)+
	LDF	(R1),AC0
	STF	AC0,(R0)
	SUB	#44.,R0		;COMPUTE T1(I,1) BY CROSSING T1(I,2)
	MOV	R0,R1		;   WITH T1(I,3)
	ADD	#12.,R1
	MOV	R1,R2
	ADD	#12.,R2
	JSR	PC,CROSS
	RTS	PC

MULSHR:	ADD	#12.,R0		;FORM COL 2,3,4 OF TRANS X TRANS
	ADD	#12.,R2
	MOV	#3,R3
	JSR	PC,MULCOL
	ADD	#44.,R1		;ADD IN T1(I,4)
	ADDF	(R1),AC0
	STF	AC0,-(R0)
	LDF	-(R0),AC0
	ADDF	-(R1),AC0
	STF	AC0,(R0)
	LDF	-(R0),AC0
	ADDF	-(R1),AC0
	STF	AC0,(R0)
	RTS	PC

MULR:	JSR	PC,MULRSH	;FORM COL 1,2,3 OF TRANS X TRANS
	SUB	#36.,R0		;COMPUTE T1(I,1) BY CROSSING T1(I,2)
	MOV	R0,R1		;   WITH T1(I,3)
	ADD	#12.,R1
	MOV	R1,R2
	ADD	#12.,R2
	JSR	PC,CROSS
	RTS	PC

MULRSH:	ADD	#12.,R0		;FORM COL 2,3 OF TRANS X TRANS
	ADD	#12.,R2
	MOV	#2,R3
	JSR	PC,MULCOL
	RTS	PC

;END OF "MATMUL"
;MULT   - PERFORMS THE OPERATION  A0 ← A1*A2

;WILL NOT WORK FOR THE CASE OF LOC(A0)=LOC(A1)
;
;	R0 ← PTR TO ARRAY FOR RESULT: A0
;	R1 ← PTR TO 3X3 MATRIX: A1
;	R2 ← PTR TO ARRAY A2
;	R3 ← NUMBER OF COLUMNS IN A0,A2
;
;AT THE END OF THE OPERATION R0,R2 ARE LEFT POINTING AT
;THE FIRST ELEMENT IN THE N+1 COLUMN
;AND AC0 CONTAINS THE LAST SUM. R1 IS LEFT UNCHANGED

MULROW:	MOV	#BYROWS,R4
	BR	MULT

MULCOL:	MOV	#BYCOLS,R4

MULT:	MOV	#3,R5		;MULT A2 BY 3 ROWS OF A1
	LDF	(R2)+,AC2	;LOAD A COLUMN OF A2
	LDF	(R2)+,AC3
	LDF	(R2)+,AC0
	STF	AC0,AC4
1$:	LDF	(R1),AC0	;A1(I,1)*A2(1,J)
	MULF	AC2,AC0
	ADD	(R4)+,R1
	LDF	(R1),AC1	;+ A1(I,2)*A2(2,J)
	MULF	AC3,AC1
	ADD	(R4)+,R1
	ADDF	AC1,AC0
	LDF	(R1),AC1	;+ A1(I,3)*A2(3,J)
	MULF	AC4,AC1
	ADD	(R4)+,R1
	MOV	(R4),R4
	ADDF	AC1,AC0
	STF	AC0,(R0)+	;SAVE A0(I,J)
	SOB	R5,1$
	SOB	R3,MULT
	RTS	PC

BYCOLS:	.WORD	14,14,-24,.+2
	.WORD	14,14,-24,.+2
	.WORD	14,14,-40,BYCOLS

BYROWS:	.WORD	4,4,4,.+2
	.WORD	4,4,4,.+2
	.WORD	4,4,-40,BYROWS

;END OF "MULT"
;CROSS  - VECTOR CROSS PRODUCT OPERATION V0 ← V1 X V2

;	R0 ← PTR TO VECTOR FOR RESULT: V0
;	R1 ← PTR TO VECTOR V1
;	R2 ← PTR TO VECTOR V2

CROSS:	LDF	(R1)+,AC0	;X1
	LDF	(R2)+,AC1	;X2
	LDF	(R2)+,AC2	;Y2
	STF	AC2,AC4
	LDF	(R1)+,AC3	;Y1
	STF	AC3,AC5
	MULF	AC0,AC2		;X1*Y2
	MULF	AC1,AC3		;X2*Y1
	SUBF	AC3,AC2		;Z0 = X1*Y2-X2*Y1
	LDF	(R2),AC3	;Z2
	MULF	AC3,AC0		;X1*Z2
	MULF	AC5,AC3		;Y1*Z2
	STF	AC0,AC5
	LDF	(R1),AC0	;Z1
	MULF	AC0,AC1		;Z1*X2
	MULF	AC4,AC0		;Z1*Y2
	SUBF	AC5,AC1		;Y0 = Z1*X2-X1*Z2
	SUBF	AC0,AC3		;X0 = Y1*Z2-Z1*Y2
	STF	AC3,(R0)+	;SAVE X0,Y0,Z0
	STF	AC1,(R0)+
	STF	AC2,(R0)
	RTS	PC

;END OF "CROSS"
;LOCAL STORAGE AREA

T36PTU:	.WORD	T311,T321,T332,T333,T312,T322,T313,T323,T331
	.WORD	T033,T023,T013,T032,T031,T022,T021,T012,T034
	.WORD	T023,T024,T013,T014

T36PTR:	.WORD	T311,T321,T332,T333,T312,T322,T313,T323,T331
	.WORD	T633,T623,T613,T632,T631,T622,T621,T612,T634
	.WORD	T623,T624,T613,T614

T63PTR:	.WORD	T611,T612,T623,T633,T621,T622,T631,T632,T613
	.WORD	T333,T332,T331,T323,T313,T322,T312,T321

T011:	.BLKW	2
T021:	.BLKW	2
T031:	.BLKW	2
T012:	.BLKW	2
T022:	.BLKW	2
T032:	.BLKW	2
T013:	.BLKW	2
T023:	.BLKW	2
T033:	.BLKW	2
T014:	.BLKW	2
T024:	.BLKW	2
T034:	.BLKW	2

T311:	.BLKW	2
T321:	.BLKW	2
T331:	.BLKW	2
T312:	.BLKW	2
T322:	.BLKW	2
T332:	.BLKW	2
T313:	.BLKW	2
T323:	.BLKW	2
T333:	.BLKW	2
T314:	.BLKW	2
T324:	.BLKW	2
T334:	.BLKW	2

CT6:
T611:	.BLKW	2
T621:	.BLKW	2
T631:	.BLKW	2
T612:	.BLKW	2
T622:	.BLKW	2
T632:	.BLKW	2
T613:	.BLKW	2
T623:	.BLKW	2
T633:	.BLKW	2
T614:	.BLKW	2
T624:	.BLKW	2
T634:	.BLKW	2

SINT6:	.BLKW	2	;SIN THETA 6
COST6:	.BLKW	2	;COS THETA 6
;PHYSICAL CONSTANTS FOR YELLOW AND BLUE ARMS

;ADDRESSES OF ARM CONSTANTS RELATIVE TO THE START OF THE FOLLOWING TABLES

S1    ==0	;JOINT 1 OFFSET
S2    ==4	;JOINT 2 OFFSET
BASEX ==10	;TABLE COORDINATES OF BASE OF ARM 
BASEY ==14
S6    ==20	;JOINT 6 OFFSET
S22   ==24	;S2**2
STOP1 ==30	;MIN. & MAX. JOINT STOPS RELATIVE TO START OF CONST. AREA
		;JOINT 2 HAS NO STOP LIMIT SINCE ALL SOLUTIONS ARE POSSIBLE
STOP3 ==40
STOP4 ==50
STOP5 ==60
STOP6 ==70
MIDDY1==100	;MID-RANGE OF MOTION
MIDDY4==104

;CONSTANT PARAMETERS FOR YELLOW ARM, **ORDERED LIST**

YCON:	.FLT2	 16.24		;JOINT 1 OFFSET
	.FLT2	  6.05		;JOINT 2 OFFSET
	.FLT2	 29.5		;TABLE COORDINATES OF ARM BASE
	.FLT2	  8.375
	.FLT2	  9.38		;JOINT 6 OFFSET
	.FLT2	 36.6025	;S2**2

;YELLOW ARM JOINT STOP LIMITS

	.FLT2	-185.0		;JOINT 1 MIN
	.FLT2	  60.0		; 	 MAX
	.FLT2	   6.5		;JOINT 3 MIN
	.FLT2	  27.5		;	 MAX
	.FLT2	-175.0		;JOINT 4 MIN
	.FLT2	 140.0		; 	 MAX
	.FLT2	-101.0		;JOINT 5 MIN
	.FLT2	 101.0		;	 MAX
	.FLT2	-360.0		;JOINT 6 MIN
	.FLT2	 360.0		;	 MAX
	.FLT2	 -62.5		;MIDDY1
	.FLT2	 -17.5		;MIDDY4
;	[BLUE ARM TABLE OF CONSTANTS]

;CONSTANT PARAMETERS FOR BLUE ARM, **ORDERED LIST**

BCON:	.FLT2	20.24		;JOINT 1 OFFSET
	.FLT2	-6.05		;JOINT 2 OFFSET
	.FLT2	29.53125	;TABLE COORDINATES OF ARM BASE
	.FLT2	50.805
	.FLT2	10.28125	;JOINT 6 OFFSET
	.FLT2	36.6025		;S2**2

;BLUE ARM JOINT STOP LIMITS

	.FLT2	 -45.0		;JOINT 1 MIN
	.FLT2	 190.0		;	 MAX
	.FLT2	   6.75		;JOINT 3 MIN
	.FLT2	  33.0		;	 MAX
	.FLT2	-395.0		;JOINT 4 MIN
	.FLT2	 205.0		; 	 MAX
	.FLT2	 -95.0		;JOINT 5 MIN 
	.FLT2	  95.0		; 	 MAX
	.FLT2	-110.0		;JOINT 6 MIN
	.FLT2	 200.0		;        MAX
	.FLT2	  72.5		;MIDDY1
	.FLT2	 -95.0		;MIDDY4

;END OF "ARMSOL"